Constrained-Mechanical-Systems-Forward-Dynamic-Simulation-of-Chain-links

Multibody dynamics modeling and numerical methods

1 Explicit calculation of the Lagrange multiplier

%matplotlib inline
import time, os
import numpy as np
import sympy as sp
from scipy.integrate import solve_ivp
from IPython.display import display, Markdown
import matplotlib as mpl
from Models.models import *
from utils.SolutionDemo import *
sp.init_printing()
mpl.rcParams['figure.dpi'] = 50
repo_dir = os.path.dirname(os.getcwd())

Double Pendulum

m=[1, 1]
l=[1, 1]
Twobar_Model = ExplictModel(m=m, l=l,close_chain=False)
# y = np.append([0.5, 0, 0, 1.5, 0, 0], np.zeros(3*len(m)))
y0 = [None, None, np.pi/6, None, None, -np.pi/6]
y_dot = [0, None, None, 0.5, None, None]
y = np.concatenate(Twobar_Model.initial_condition(y0, y_dot))
g = np.tile([0, -9.81, 0], len(m))
f = [0, 0, 0, 0, 0, 0]

k = np.tile([1e6], 4)
Twobar_PModel = ApproximateModel(m=m, l=l, k=k, close_chain=False)

print_constrains(Twobar_Model)
print_govs(Twobar_Model, f)

System constrains:

$\displaystyle \operatorname{x_{c1}}{\left(t \right)} - 0.5 \cos{\left(\theta_{1}{\left(t \right)} \right)} = 0$

$\displaystyle \operatorname{y_{c1}}{\left(t \right)} - 0.5 \sin{\left(\theta_{1}{\left(t \right)} \right)} = 0$

$\displaystyle \operatorname{x_{c2}}{\left(t \right)} - \cos{\left(\theta_{1}{\left(t \right)} \right)} - 0.5 \cos{\left(\theta_{2}{\left(t \right)} \right)} = 0$

$\displaystyle \operatorname{y_{c2}}{\left(t \right)} - \sin{\left(\theta_{1}{\left(t \right)} \right)} - 0.5 \sin{\left(\theta_{2}{\left(t \right)} \right)} = 0$

System governing equations

$\displaystyle 1.0 \frac{d^{2}}{d t^{2}} \operatorname{x_{c1}}{\left(t \right)} = - 1.0 g_{1} + \lambda_{1}$

$\displaystyle 1.0 \frac{d^{2}}{d t^{2}} \operatorname{y_{c1}}{\left(t \right)} = - 1.0 g_{2} + \lambda_{2}$

$\displaystyle 0.0833333333333333 \frac{d^{2}}{d t^{2}} \theta_{1}{\left(t \right)} = - 0.0833333333333333 g_{3} + 0.5 \lambda_{1} \sin{\left(\theta_{1}{\left(t \right)} \right)} - 0.5 \lambda_{2} \cos{\left(\theta_{1}{\left(t \right)} \right)} + \lambda_{3} \sin{\left(\theta_{1}{\left(t \right)} \right)} - \lambda_{4} \cos{\left(\theta_{1}{\left(t \right)} \right)}$

$\displaystyle 1.0 \frac{d^{2}}{d t^{2}} \operatorname{x_{c2}}{\left(t \right)} = - 1.0 g_{4} + \lambda_{3}$

$\displaystyle 1.0 \frac{d^{2}}{d t^{2}} \operatorname{y_{c2}}{\left(t \right)} = - 1.0 g_{5} + \lambda_{4}$

$\displaystyle 0.0833333333333333 \frac{d^{2}}{d t^{2}} \theta_{2}{\left(t \right)} = - 0.0833333333333333 g_{6} + 0.5 \lambda_{3} \sin{\left(\theta_{2}{\left(t \right)} \right)} - 0.5 \lambda_{4} \cos{\left(\theta_{2}{\left(t \right)} \right)}$

ode = 'DOP853'
sol = solve_ivp(Twobar_Model.sim, [0, 10], y, method=ode, args=(f, g), t_eval=np.linspace(0, 10, 200))
Twobar = SolutionDemo(sol, m, l, rot=None)
multipliers = get_multipliers(Twobar_Model, f, g, sol, show=False)
print(Twobar.links.shape)

(2, 200, 2, 2)

ode = 'DOP853'
solp = solve_ivp(Twobar_PModel.sim, [0, 10], y, method=ode, args=(f, g), t_eval=np.linspace(0, 10, 200))
Twobarp = SolutionDemo(solp, m, l, rot=None)
print(Twobarp.links.shape)

# Twobar.animate(title=ode, interval=50, save_as=repo_dir+'/imgs/DoublePendulum.gif')

Twobar.more_anim(multipliers, title='Double Pendulum', interval=50, save_as=repo_dir+'/imgs/DoublePendulum.mp4')

Once Loop Reflect

# Twobarp.animate(title=ode)

# Twobar.plot_angles()

# Twobarp.plot_angles()